#include <windows.h>
#include "MSR_NuiApi.h"
#include <stdio.h>
#include <strsafe.h>
#include <math.h>
#include <time.h>
#include "SDL.h"
#include "SDL_opengl.h"

//Events ...
HANDLE m_hNextDepthFrameEvent;
HANDLE m_hNextVideoFrameEvent;
HANDLE m_hNextSkeletonEvent;

//EventHandle
HANDLE m_pVideoStreamHandle;
HANDLE m_pDepthStreamHandle;

// thread handling
HANDLE m_hThNuiProcess;
HANDLE m_hEvNuiProcessStop;

const int SCREEN_WIDTH = 400;
const int SCREEN_HEIGHT = 400;
const int SCREEN_BPP = 32;

/*
DWORD WINAPI Nui_ProcessThread(LPVOID pParam)
{
	int nEventIdx=-200;
	const NUI_IMAGE_FRAME * pImageFrame = NULL;

	//HANDLE hEvents[4];
	//hEvents[0]=m_hEvNuiProcessStop;
 //   hEvents[1]=m_hNextDepthFrameEvent;
 //   hEvents[2]=m_hNextVideoFrameEvent;
 //   hEvents[3]=m_hNextSkeletonEvent;
	HANDLE hEvents[2];
	hEvents[0]=m_hEvNuiProcessStop;
    hEvents[1]=m_hNextVideoFrameEvent;
	while(1)
	{
		nEventIdx=WaitForMultipleObjects(sizeof(hEvents)/sizeof(hEvents[0]),hEvents,FALSE,100);
		if(nEventIdx==0)
			break;
		else
		{
			if(nEventIdx==WAIT_OBJECT_0 || nEventIdx==WAIT_ABANDONED_0 || nEventIdx==WAIT_TIMEOUT || nEventIdx==WAIT_FAILED)
			{
				int i =0;
			}
			if(nEventIdx==WAIT_OBJECT_0)
			{
				int i =0;
			}
			if(nEventIdx==WAIT_ABANDONED_0)
			{
				int i =0;
			}
			if(nEventIdx==WAIT_TIMEOUT)
			{
				int i =0;
			}
			if(nEventIdx==WAIT_FAILED)
			{
				LPTSTR lpszFunction=TEXT("GetProcessId");
				 LPVOID lpMsgBuf;
				LPVOID lpDisplayBuf;
				DWORD dw = GetLastError();
				FormatMessage(
					FORMAT_MESSAGE_ALLOCATE_BUFFER | 
					FORMAT_MESSAGE_FROM_SYSTEM |
					FORMAT_MESSAGE_IGNORE_INSERTS,
					NULL,
					dw,
					MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
					(LPTSTR) &lpMsgBuf,
					0, NULL );

				// Display the error message and exit the process

				lpDisplayBuf = (LPVOID)LocalAlloc(LMEM_ZEROINIT, 
					(lstrlen((LPCTSTR)lpMsgBuf) + lstrlen((LPCTSTR)lpszFunction) + 40) * sizeof(TCHAR)); 
				StringCchPrintf((LPTSTR)lpDisplayBuf, 
					LocalSize(lpDisplayBuf) / sizeof(TCHAR),
					TEXT("%s failed with error %d: %s"), 
					lpszFunction, dw, lpMsgBuf); 
				MessageBox(NULL, (LPCTSTR)lpDisplayBuf, TEXT("Error"), MB_OK); 

				LocalFree(lpMsgBuf);
				LocalFree(lpDisplayBuf);
				ExitProcess(dw); 
			}
			
			switch(nEventIdx)
			{
				case 1:
				{
					break;
				}
				case 2:
				{
					NuiImageStreamGetNextFrame(m_pVideoStreamHandle, 0, &pImageFrame );
					NuiImageStreamReleaseFrame(m_pVideoStreamHandle, pImageFrame);
					break;
				}
				case 3:
				{
					break;
				}
			}
		}
	}
	return (0);
}
*/

bool init_GL(){
	glClearColor(0.0f,0.0f,0.0f,0.0f);
	glMatrixMode( GL_PROJECTION );
	glLoadIdentity();
	glOrtho(0,SCREEN_WIDTH,SCREEN_HEIGHT,0, -1, 1);

	glMatrixMode( GL_MODELVIEW );
	glLoadIdentity();

	if( glGetError() != GL_NO_ERROR )
		return false;

	return true;
}
bool init(){
	if( SDL_Init( SDL_INIT_EVERYTHING) < 0 )
		return false;

	if(SDL_SetVideoMode(SCREEN_WIDTH, SCREEN_HEIGHT, SCREEN_BPP, SDL_OPENGL) == NULL)
		return false;

	if(!init_GL())
		return false;

	SDL_WM_SetCaption("Kinect",NULL);

	return true;
}
void clean_up()
{
    //Quit SDL
    SDL_Quit();
}
void drawCircles(int hrx, int hry, int hrz, int hlx, int hly, int hlz){

	int rrad = hrz*2+10;
	int lrad = hlz*2+10;

	glBegin(GL_TRIANGLE_FAN);
		glColor4f(1.0f,0.0f,0.0f,1.0f);
		glVertex2f(hrx,hry);
		for (int i = 0; i < 360; i+=5){
			glVertex2f(hrx + sin((float)i)*rrad, hry + (float)cos((float)i)*lrad);
		}
	glEnd();

	glBegin(GL_TRIANGLE_FAN);
		glColor4f(0.0f,1.0f,0.0f,1.0f);
		glVertex2f(hlx,hly);
		for (int i = 0; i < 360; i+=5){
		glVertex2f(hlx + sin((float)i)*rrad, hly + cos((float)i)*lrad);
		}
	glEnd();
}

int main(int argc, char *argv[])
{
	NUI_SKELETON_FRAME SkeletonFrame;

	NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_SKELETON | NUI_INITIALIZE_FLAG_USES_COLOR);
	m_hNextVideoFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL);
	m_hNextDepthFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL);
	m_hNextSkeletonEvent = CreateEvent( NULL, TRUE, FALSE, NULL);
	NuiSkeletonTrackingEnable(m_hNextSkeletonEvent, 0);
	HRESULT hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, m_hNextVideoFrameEvent, &m_pVideoStreamHandle);
	NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_320x240, 0, 2, m_hNextDepthFrameEvent, &m_pDepthStreamHandle);
	HANDLE m_hEvNuiProcessStop = CreateEvent(NULL, FALSE, FALSE, NULL);

	int nEventIdx;
	const NUI_IMAGE_FRAME * pImageFrame = NULL;
	HANDLE hEvents[4];
	hEvents[0]=m_hEvNuiProcessStop;
    hEvents[1]=m_hNextDepthFrameEvent;
    hEvents[2]=m_hNextVideoFrameEvent;
    hEvents[3]=m_hNextSkeletonEvent;

	float hlx, hly, hlz, hrx, hry, hrz;
	hlx = hly = hlz = hrx = hry = hrz = 0;
	time_t seconds;
	time_t pseconds;

    pseconds = time (NULL);

	if(!init())
		return 1;

	SDL_Event event;
	bool isDone = false;

	while(!isDone)
	{
		while( SDL_PollEvent( &event ) )
        {
			//if person quits
            if( event.type == SDL_QUIT )
                isDone = true;
		}

		NuiSkeletonGetNextFrame(0, &SkeletonFrame);
		for( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ )
		{
			if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED )
			{
				//bFoundSkeleton = false;   
				//printf(" Found skeleton");

   
				// TIMES 200 so that it scales to 400x400 screen
				hlx=SkeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT].x * 100 + 200;
				hly=SkeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT].y * 100;
				hlz=SkeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT].z;   
	
				//printf("x: %f, y: %f, z: %f \n", hand_l_x, hand_l_y, hand_l_z);
				//right hand
				hrx=SkeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT].x * 100 + 200;
				hry=SkeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT].y * 100 + 200;
				hrz=SkeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT].z;

				/*
				seconds=time(NULL);
				if(seconds-pseconds>=1)
				{
					printf("Left Hand: %f, %f, %f    Right Hand: %f, %f, %f\n", hand_l_x, hand_l_y, hand_l_z, hand_r_x, hand_r_y, hand_r_z);
					pseconds=seconds;
				}
				
				if (fabs(pHandR_x-hand_r_x) > .1)
					printf("Right Hand: %f, %f, %f \n", hand_l_x, hand_l_y, hand_l_z);
				if(fabs(pHandL_x-hand_l_x) > .1)
					printf("Left Hand: %f, %f, %f \n", hand_r_x, hand_r_y, hand_r_z);
				

				pHandR_x = hand_r_x;
				pHandR_y = hand_r_y;
				pHandR_z = hand_r_z;

				pHandL_z = hand_l_z;
				pHandL_x = hand_l_x;
				pHandL_y = hand_l_y;

				//if (hand_l_x!=0 && hand_l_y!=0)
				if (hand_l_x!=0 && hand_r_z!=0 )
				{
					//printf("HandLeft X=%4.2f    ",hand_l_x,"HandLeft Y=%4.2f    ",hand_l_y,"HandLeft Z=%4.2f    \r\n",hand_l_z);
					//printf("HandRight X=%4.2f    ",hand_r_x,"HandRight Y=%4.2f    ",hand_r_y,"HandRight Z=%4.2f    \r\n",hand_r_z);   
					printf("HandLeft X=%4.2f    ",hand_l_x,"HandLeft Y=%4.2f    ",hand_l_y,"HandLeft Z=%4.2f    \r",hand_l_z);
					printf("HandRight X=%4.2f    ",hand_r_x,"HandRight Y=%4.2f    ",hand_r_y,"HandRight Z=%4.2f    \r",hand_r_z);   
				}
				*/
			}
		}	
		/*
		nEventIdx=WaitForMultipleObjects(sizeof(hEvents)/sizeof(hEvents[0]),hEvents,FALSE,100);
		printf("%d", nEventIdx);
		if(nEventIdx==0)
			break;
		else
		{
			if(nEventIdx==WAIT_OBJECT_0)
			{
				int i =0;
			}
			if(nEventIdx==WAIT_ABANDONED_0)
			{
				int i =0;
			}
			if(nEventIdx==WAIT_TIMEOUT)
			{
				int i =0;
			}
			if(nEventIdx==WAIT_FAILED)
			{
				LPTSTR lpszFunction=TEXT("GetProcessId");
				LPVOID lpMsgBuf;
				LPVOID lpDisplayBuf;
				DWORD dw = GetLastError();
				FormatMessage(
					FORMAT_MESSAGE_ALLOCATE_BUFFER | 
					FORMAT_MESSAGE_FROM_SYSTEM |
					FORMAT_MESSAGE_IGNORE_INSERTS,
					NULL,
					dw,
					MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
					(LPTSTR) &lpMsgBuf,
					0, NULL );

				// Display the error message and exit the process

				lpDisplayBuf = (LPVOID)LocalAlloc(LMEM_ZEROINIT, 
					(lstrlen((LPCTSTR)lpMsgBuf) + lstrlen((LPCTSTR)lpszFunction) + 40) * sizeof(TCHAR)); 
				StringCchPrintf((LPTSTR)lpDisplayBuf, 
					LocalSize(lpDisplayBuf) / sizeof(TCHAR),
					TEXT("%s failed with error %d: %s"), 
					lpszFunction, dw, lpMsgBuf); 
				MessageBox(NULL, (LPCTSTR)lpDisplayBuf, TEXT("Error"), MB_OK); 

				LocalFree(lpMsgBuf);
				LocalFree(lpDisplayBuf);
				ExitProcess(dw); 
			}
			
			switch(nEventIdx)
			{
				case 1:
				{
					break;
				}
				case 2:
				{
					NuiImageStreamGetNextFrame(m_pVideoStreamHandle, 0, &pImageFrame );
					NuiImageStreamReleaseFrame(m_pVideoStreamHandle, pImageFrame);
					break;
				}
				case 3:
				{
					break;
				}
			}
		}
		*/

		//clear image from screen
		glClear( GL_COLOR_BUFFER_BIT );
		//draw the hand positions
		drawCircles(hrx,hry,hrz,hlx,hly,hlz);
		//Update screen
		SDL_GL_SwapBuffers();

	}
	
	NuiShutdown();
	clean_up();
	return 0;	
}

//int main(int argc, char *argv[])
//{
//	HANDLE m_hNextVideoFrameEvent;
//	HANDLE m_pVideoStreamHandle, m_pDepthStreamHandle, m_hNextDepthFrameEvent, m_hNextSkeletonEvent;
//	HRESULT hr;
//	m_hNextVideoFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL);
//	m_hNextDepthFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL);
//	m_hNextSkeletonEvent = CreateEvent( NULL, TRUE, FALSE, NULL);
//	HANDLE m_hEvNuiProcessStop = CreateEvent(NULL, FALSE, FALSE, NULL);
//	//HANDLE m_hThNuiProcess = CreateThread(NULL, 0, Nui_ProcessThread, this, 0, NULL);
//	//NuiSkeletonTrackingEnable(m_hNextSkeletonEvent, 0);
//
//	NuiInitialize( 0x00000001 | 0x00000008 | 0x00000002);
//	hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, m_hNextVideoFrameEvent, &m_pVideoStreamHandle);
//	NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_320x240, 0, 2, m_hNextDepthFrameEvent, &m_pDepthStreamHandle);
//
//	int nEventIdx;
//	HANDLE hEvents[4];
//	hEvents[0]=m_hEvNuiProcessStop;
//    hEvents[1]=m_hNextDepthFrameEvent;
//    hEvents[2]=m_hNextVideoFrameEvent;
//    hEvents[3]=m_hNextSkeletonEvent;
//	while(1){
//		nEventIdx=WaitForMultipleObjects(sizeof(hEvents)/sizeof(hEvents[0]),hEvents,FALSE,100);
//		
//		printf("%d", nEventIdx);
//		if(nEventIdx==0){
//            break;
//		}
//		switch(nEventIdx){
//		case 1:
//			break;
//		case 2:
//			break;
//		case 3:
//			break;
//		}
//	}
//	NuiShutdown();
//	return 0;
//}







//#include <windows.h>
//#include "MSR_NuiApi.h"
//#include <stdio.h>
//
////Events ...
//HANDLE m_hNextDepthFrameEvent;
//HANDLE m_hNextVideoFrameEvent;
//HANDLE m_hNextSkeletonEvent;
//
////EventHandle
//HANDLE m_pVideoStreamHandle;
//HANDLE m_pDepthStreamHandle;
//
//// thread handling
//HANDLE m_hThNuiProcess;
//HANDLE m_hEvNuiProcessStop;
//
//
//int main(int argc, char* argv){
//
//	HRESULT    hr;
// m_hNextDepthFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
// m_hNextVideoFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
// m_hNextSkeletonEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
//
// hr = NuiInitialize( 
//  NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_SKELETON | NUI_INITIALIZE_FLAG_USES_COLOR);
// if( FAILED( hr ) ) printf("Initialize verkackt \ n");
// 
//	hr = NuiSkeletonTrackingEnable( m_hNextSkeletonEvent, 0 );
// if( FAILED( hr ) ) printf("TrackingEnable verkackt \n");
// 
//	 hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480,
//							 0, 2, m_hNextVideoFrameEvent, &m_pVideoStreamHandle );
// if( FAILED( hr ) ) printf("VideoStreamHandle verkackt \n");
// 
//
// hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_320x240,
//							0, 2, m_hNextDepthFrameEvent, &m_pDepthStreamHandle );
// if( FAILED( hr ) ) printf("DepthStreamHanle verkackt \n");
// 
// HANDLE	hEvents[4];
// int		nEventIdx;
//
//	// Configure events to be listened on
// hEvents[0] = m_hEvNuiProcessStop;
// hEvents[1] = m_hNextDepthFrameEvent;
// hEvents[2] = m_hNextVideoFrameEvent;
// hEvents[3] = m_hNextSkeletonEvent;
//	
//	// Main thread loop
// while (1)
//{
//  const NUI_IMAGE_FRAME * pImageFrame = NULL;
//  HRESULT hr = NuiImageStreamGetNextFrame(m_pVideoStreamHandle, 200, &pImageFrame);
//  if( hr == E_NUI_FRAME_NO_DATA)
//  {
//    // No stream frame was found after waiting for
//    // requested amount. Continue and try again (or, you could
//    // also choose to bail out from function here).
//    continue;
//  }
//  else if (FAILED(hr))
//  {
//    break;
//  }
//
//  // Your code to deal with data from pImageFrame
//}
// return hr;
//}